/*
 * direction.c
 *
 *  Created on: May 22, 2021
 *      Author: kirk_
 */

#include "direction.h"

#include "velocity.h"

void Turn(float a)
{
	Direction.Target += a;
}

void DirectionInit(void* argument)
{
	uint32_t Period;

	(void)Period;

	/* Get arg */
	if(argument != NULL)
		Period = *(uint32_t*)argument;
	else
		Period = Default_Fetch_Period;

	/* Init params */
	KD_FloatPID_Init(&Direction, DirectionDownlink);
	KD_FloatPID_SetIndex(&Direction, KD_FloatPID_IndexP, 0.00);
	KD_FloatPID_SetIndex(&Direction, KD_FloatPID_IndexI, 0.00);
	KD_FloatPID_SetIndex(&Direction, KD_FloatPID_IndexD, 0.00);
	KD_FloatPID_SetIndex(&Direction, KD_FloatPID_Target, 0.00);
	KD_FloatPID_SetIndex(&Direction, KD_FloatPID_Tolerance, 0.00);

	/* Test param */
	Direction.Denominator = -1000;
	Direction.indexP = 0.20;
	Direction.indexI = 0.00;
	Direction.indexD = 0.00;
	Direction.Tolerance = 0.00;
	Direction.Target = 0.00;

	return;
}

void DirectionDownlink(KD_FloatPID_t* pid)
{
	if(&Direction == pid)
	{
		VelocityIncTarget(0, pid->Increment);
		VelocityIncTarget(1, -pid->Increment);
		VelocityIncTarget(2, pid->Increment);
		VelocityIncTarget(3, -pid->Increment);
	}

	return;
}


/* Thread */

volatile double Yaw = 0;

void StartDirectionThread(void *argument)
{
	uint32_t Period;
	int i = 0;
	double dtmp;
	double GyrZ_Offset;

	/* Check Arg */
	if(argument != NULL)
		Period = *(uint32_t*)argument;
	else
		Period = Default_Direction_Period;

	/* Start timer */
	osTimerStart(DirectionTimerHandle, Period);
	dtmp = 0;

	/* Wait a while */
	osDelay(3);

	/* Erase error */
	for(i=0;i<Direction_Erase_Error_Times;i++)
	{
		osThreadSuspend(DirectionThreadHandle);
		/* Fetch mpu6050 */
		KDL_MPU6050_Get_Gyroscope(&MPU6050);
		/* Accumulate */
		dtmp += MPU6050.GyrZ;
		/* log */
		PRINTF("[MPU6050] Acc %d times in %d.\r\n", i, Direction_Erase_Error_Times);
	}
	/* Count offset */
	dtmp /= Direction_Erase_Error_Times;
	GyrZ_Offset = dtmp;
	/* Log */
	PRINTF("[MPU6050] Init OK with offset %ld.\r\n", (int32_t)(GyrZ_Offset*1000));
	/* Infinite loop */
	for(;;)
	{
		osThreadSuspend(DirectionThreadHandle);
		/* Fetch mpu6050 */
		KDL_MPU6050_Get_Gyroscope(&MPU6050);
		MPU6050.GyrZ -= GyrZ_Offset;
		Yaw += MPU6050.GyrZ * Period;
#if 0
		PRINTF("[GyrZ] %ld;\r\n", (int32_t)(MPU6050.GyrZ * 1000));
		PRINTF("[Yaw] %ld;\r\n", (int32_t)(Yaw));
#endif
	}
}

float DirectionGet(void)
{
	return (float)Yaw/1000.00;
}

void DirectionTimerCallback(void *argument)
{
	/* USER CODE BEGIN DirectionTimerCallback */
	osThreadResume(DirectionThreadHandle);
	/* USER CODE END DirectionTimerCallback */
}
